#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy
from mqtt_integration.msg import TrajectoryInfo, TimeInfo
import paho.mqtt.client as mqtt
import time
import hmac
import hashlib
import signal
import sys
from datetime import datetime

# 全局变量
current_trajectory = 0
total_executions = 0
current_time = ""
elapsed_time = ""

def trajectory_info_callback(msg):
    global current_trajectory, total_executions
    current_trajectory = msg.current_trajectory
    total_executions = msg.total_executions

def time_info_callback(msg):
    global current_time, elapsed_time
    current_time = str(msg.current_time)
    elapsed_time = str(msg.elapsed_time)

def main():
    global current_trajectory, total_executions, current_time, elapsed_time
    
    # ROS节点初始化
    rospy.init_node('mqtt_publisher')
    rospy.Subscriber('trajectory_info', TrajectoryInfo, trajectory_info_callback)
    rospy.Subscriber('time_info', TimeInfo, time_info_callback)

    # xxyl配置
    broker = "30l8a75806.qicp.vip"
    port = 57254
    topic = "/iot/test901/realtimeData"
    username = "vipxyyl"
    password = "vipxyyl2022"
    client_id = "test901"

    # 创建 MQTT 客户端
    client = mqtt.Client(client_id)

    # 设置用户名和密码
    client.username_pw_set(username, password)

    # 定义连接回调
    def on_connect(client, userdata, flags, rc):
        if rc == 0:
            print("连接成功")
            client.subscribe(topic)
        else:
            print(f"连接失败，返回码: {rc}")

    # 定义消息接收回调
    def on_message(client, userdata, msg):
        print(f"收到消息: {msg.topic} {msg.payload}")

    # 绑定回调函数
    client.on_connect = on_connect
    # client.on_message = on_message

    # 定义日志记录回调
    def on_log(client, userdata, level, buf):
        if level == mqtt.MQTT_LOG_ERR:
            print(f"错误: {buf}")
        elif level == mqtt.MQTT_LOG_WARNING:
            print(f"警告: {buf}")
        elif level == mqtt.MQTT_LOG_INFO:
            print(f"信息: {buf}")
        elif level == mqtt.MQTT_LOG_NOTICE:
            print(f"通知: {buf}")

    client.on_log = on_log

    # 连接到服务器
    client.connect(broker, port, 60)

    # 循环处理网络流
    client.loop_start()

    # 捕获Ctrl+C
    def signal_handler(sig, frame):
        print("正在停止...")
        client.loop_stop()
        client.disconnect()
        sys.exit(0)

    signal.signal(signal.SIGINT, signal_handler)

    # 发送示例协议数据
    try:
        while not rospy.is_shutdown():
            # 在每次循环开始时重置时间信息
            current_time = ""
            elapsed_time = ""

            # 小延迟以等待可能的信息更新
            time.sleep(1)

            if isinstance(current_time, str) and isinstance(elapsed_time, str) and current_time and elapsed_time:
                # 提取当前时间的年月日
                current_time_dt = datetime.strptime(current_time, "%Y%m%d%H%M%S")
                year = current_time_dt.year % 100  # 取两位数年份
                month = current_time_dt.month
                day = current_time_dt.day
                hour = current_time_dt.hour
                minute = current_time_dt.minute

                # 提取经过时间的小时分钟秒
                elapsed_time_td = datetime.strptime(elapsed_time, "%H:%M:%S")
                hours = elapsed_time_td.hour
                minutes = elapsed_time_td.minute
                seconds = elapsed_time_td.second

                # 创建字节数组
                data = bytearray([
                    0x0d, 0x01,
                    current_trajectory & 0xFF, total_executions & 0xFF,
                    year, month, day, hour, minute,
                    hours & 0xFF, minutes & 0xFF, seconds & 0xFF,
                    0xcd, 0xdc
                ])

            else:
                # 发送默认数据
                data = bytearray([
                    0x0d, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
                    0x00, 0x00, 0x00, 0x00, 0x00, 0xcd, 0xdc
                ])
                print("设备离线……发送默认数据")

            hex_data = data.hex()
            client.publish(topic, payload=hex_data, qos=0, retain=False)
            print(f"已发送数据: {hex_data}")
            time.sleep(10)
            
    except KeyboardInterrupt:
        signal_handler(None, None)

if __name__ == "__main__":
    main()
